// Object which holds pathfinding grid and is used to search
// for paths from point A to point B
// Nick Kitten

#include <cmath>
#include <cassert>
#include "Pathfinder.h"
#include "DarkGDK.h"

#define DEBUG

using namespace std;

extern int navigationTime, constructTime, smoothTime;

///////////////////////////////////////
// checks between two points and all points in between to see if occupied or not.
bool Pathfinder::isWalkable(Triple begin, Triple end )
{
	float distance = getDistance( begin, end );
	int numSteps = (int) ceil( distance / this->delta );

	int i, j;
	Triple currPos;
	Triple offsetPos;
	Position currIndex[5];

	for( i = 0; i < numSteps; i++ )
	{
		currPos = interpolate( begin, end, i / (float) numSteps );
		currIndex[0] = posToGrid( currPos );
		offsetPos = Triple( currPos.x + radius, currPos.y, currPos.z );
		currIndex[1] = posToGrid( offsetPos );
		offsetPos = Triple( currPos.x - radius, currPos.y, currPos.z );
		currIndex[2] = posToGrid( offsetPos );
		offsetPos = Triple( currPos.x, currPos.y, currPos.z + radius );
		currIndex[3] = posToGrid( offsetPos );
		offsetPos = Triple( currPos.x, currPos.y, currPos.z - radius );
		currIndex[4] = posToGrid( offsetPos );

		for( j = 0; j < 5; j++ )
		{
			if( !occupancyGrid->isValid( currIndex[j].i, currIndex[j].j ) )
			{
				continue;
			}
			if ( (*occupancyGrid)( currIndex[j].i, currIndex[j].j ) )
			{
				return false;
			}
		}
	}

	return true;
}

void Pathfinder::smoothPath( list<Triple> & path )
{
	int time = dbTimer();
	assert( path.size() != 0 );
	if( path.size() == 1 || path.size() == 2 )
		return;

	list<Triple>::iterator head = path.begin();
	list<Triple>::iterator next( head ); 
	list<Triple>::iterator tail( next );
	tail++;
	
	while( tail != path.end() )
	{
		head = next;
		next = tail;
		tail++;

		while( tail != path.end() && isWalkable( *head, *tail ) )
		{
			if( next != head )
				path.erase( next );
			next = tail;
			tail++;
		}
	}

	smoothTime = dbTimer() - time;
}

Position Pathfinder::posToGrid(Triple real)
{
	real.x = real.x / sideLength + 0.5f;
	real.z = real.z / sideLength + 0.5f;
	Position pos;
	pos.i = (int)real.x;
	pos.j = (int)real.z;
	return pos;
}

Triple Pathfinder::gridToPos( Position pos )
{
	//int halfLength = sideLength / 2;
	return Triple( pos.i * sideLength, 0, pos.j * sideLength );
}

void Pathfinder::clearLists()
{
	int i, j;
	
	for( i = 0; i < closedList.width(); i++ )
	{
		for( j = 0; j < closedList.height(); j++ )
		{
			closedList( i, j ) = false;
			onOpenList( i, j ) = false;
		}
	}

	while( ! openList.empty() )
	{
		openList.pop();
	}
}

Pathfinder::Pathfinder()
{
}

Pathfinder::~Pathfinder()
{
}

Pathfinder::Pathfinder( const Grid<char> * occGrid, int sLength, Algorithm m, 
					   Heuristic h, LimitMode lim, int maxLim, bool shorten, float del, float r)
					  : method(m), heuristic(h), limitMode(lim), smooth(shorten), delta(del),
					  occupancyGrid( occGrid ), sideLength( sLength ), maxLimit( maxLim ), radius( r )
{
	mapNodes = Grid<Node>::Grid( occGrid->width(), occGrid->height(), Node() );
	int i, j;

	for( i = 0; i < mapNodes.width(); i++ )
	{
		for( j = 0; j < mapNodes.height(); j++ )
		{
			mapNodes( i, j ).setPosition( Position( i, j ) );
		}
	}

	closedList = Grid<bool>( occGrid->width(), occGrid->height(), false );
	onOpenList = Grid<bool>( occGrid->width(), occGrid->height(), false );
}

void Pathfinder::loadWaypoints( const Grid<char> * wayPointMap, const Grid<char> * routingTable, 
							   const vector<Triple> * wayPointPositions )
{
	this->wayPointMap = wayPointMap;
	this->routingTable = routingTable;
	this->wayPointPositions = wayPointPositions;
}

// Constructs a list of real-valued positions ordered from start to finish, but not including
// the start node position, after A* has finished running.
void Pathfinder::constructPath( const Node * start, const Node * goal, list<Triple> & path )
{
	const Node * currNode = goal;
	Position pos;

	#ifdef DEBUG
		int time = dbTimer(), timeElapsed = 0;
	#endif

	while( currNode->getPosition() != start->getPosition() )
	{
		pos = currNode->getPosition();
		path.push_front( gridToPos( pos ) );
		currNode = currNode->Parent();

		#ifdef DEBUG
			timeElapsed = dbTimer() - time;
		#endif
	}

	#ifdef DEBUG
		constructTime = timeElapsed;
	#endif
}

int Pathfinder::dist( const Position & a, const Position & b )
{
	int dx = a.i - b.i;
	int dy = a.j - b.j;

	switch( heuristic )
	{
	case Manhattan:
		return abs( dx ) + abs( dy );
		break;
	case Euclidean:
		return dx * dx + dy * dy;
		break;
	default:
		assert(0);
		return 0;
	}
}

void Pathfinder::runAStar( Triple start, Triple goal, list<Triple> & path, int startTime )
{
	clearLists();

	int elapsedTime = 0;

	Position startInd = posToGrid( start );
	Position goalInd = posToGrid( goal );

	// check for movement within a square
	if( goalInd == startInd )
	{
		path.push_back( goal );
		return;
	}

	Position currInd;
	Node & startNode = mapNodes( startInd.i, startInd.j );
	startNode.G( 0 );

	int g, h, iter = 0;

	h = dist( startInd, goalInd );
	
	startNode.H( h );
	openList.push( startNode );
	onOpenList( startInd.i, startInd.j ) = true;
	//onOpenList( startInd.i, startInd.j ) = true;

	const Node * bestSoFar = &startNode;

	const Node & goalNode = mapNodes( goalInd.i, goalInd.j );

	Node currNode;
	Node * neighbor = 0;

	while( !openList.empty() )
	{
		currNode = openList.top();
		openList.pop();
		currInd = currNode.getPosition();

		if( bestSoFar->H() > currNode.H() )
			bestSoFar = & mapNodes( currInd.i, currInd.j );

		if( limitMode == Pathfinder::Depth )
		{
			if( ++iter >= maxLimit )
			{
				break;
			}
		}
		else if( limitMode == Pathfinder::Time )
		{
			elapsedTime = dbTimer() - startTime;

			if( elapsedTime >= maxLimit )
			{
				break;
			}
		}

		if( currNode.getPosition() == goalInd )
		{
			bestSoFar = & goalNode;
			path.push_back( goal );
			break;
		}

		closedList( currInd.i, currInd.j ) = true;

		int i, j;
		Position prospect;

		// 8 neighbor search

		for( i = -1; i <= 1; i++ )
		{
			for( j = -1; j <= 1; j++ )
			{
				// ignore center 
				if( i == j && j == 0 )
					continue;

				prospect = Position( currInd.i + i, currInd.j + j );

				if( occupancyGrid->isValid( prospect.i, prospect.j ) 
					&& !( * occupancyGrid )( prospect.i, prospect.j )
					&& !closedList( prospect.i, prospect.j ) )
				{
					// diagonals give g = 14, sides are g = 10 with Manhattan
					switch( heuristic )
					{
					case Manhattan:
						g = currNode.G() + ( ( i * j ) ? 14 : 10 );
						break;
					case Euclidean:
						g = currNode.G() + ( ( i * j ) ? 20 : 10 );
						break;
					default:
						break;
					}

					assert( g >= 0 );

					neighbor = & mapNodes( prospect.i, prospect.j );

					if( ! onOpenList( prospect.i, prospect.j ) || g < neighbor->G() )
					{
						neighbor->G( g );
						neighbor->Parent( & mapNodes( currInd.i, currInd.j ) );
						neighbor->H( dist( prospect, goalInd ) );
						openList.push( * neighbor );
						onOpenList( prospect.i, prospect.j ) = true;
					}
				}
			}
		}

	}


	constructPath( &startNode, bestSoFar, path );

		// check for movement within a square
	/*
	if( bestSoFar->getPosition() == startInd )
	{
		path.push_back( start );
	}
	*/
	//assert( ! path.empty() );

}

void Pathfinder::runWaypoints( Triple start, Triple goal, list<Triple> & path, int startTime )
{

	Position startInd = posToGrid( start );
	Position goalInd = posToGrid( goal );

	char startNode = (*wayPointMap)( startInd.x, startInd.z );
	char goalNode = (*wayPointMap)( goalInd.x, goalInd.z);

	assert( goalNode >= 'a' && goalNode <= 'z' );

	// check for 

	while( startNode != ' ' )
	{
		if( limitMode == Pathfinder::Time )
		{
			assert( dbTimer() - startTime < maxLimit );
		}

		path.push_back( (*wayPointPositions)[ startNode - 'a' ] );
		startNode = (*routingTable)( startNode - 'a', goalNode - 'a' );
	}

	path.push_back( goal );
}

list<Triple> Pathfinder::navigate( Triple start, Triple goal )
{
	int startTime, elapsedTime;

	startTime = dbTimer();

	list<Triple> path;
	//path.push_back( start );

	switch( method )
	{
	case AStar:
		runAStar( start, goal, path, startTime );
		break;
	
	case Waypoints:
		runWaypoints( start, goal, path, startTime );
		break;
	}

	path.push_front( start );

	#ifdef DEBUG
		navigationTime = dbTimer() - startTime;
	#endif

	if( smooth )
		smoothPath( path );

	return path;
}

void Pathfinder::setAlgorithm( Algorithm method )
{
	this->method = method;
}

void Pathfinder::setHeuristic( Heuristic heur )
{
	this->heuristic = heur;
}

void Pathfinder::setDelta( float newDelta )
{
	this->delta = newDelta;
}

void Pathfinder::setRadius( float newRadius )
{
	this->radius = newRadius;
}

void Pathfinder::setLimitMode( LimitMode mode )
{
	this->limitMode = mode;
}

void Pathfinder::setSmooth( bool smth )
{
	this->smooth = smth;
}